#include <virt_io.h>
#include <asm/pl_uart.h>
#include <error.h>

static int check_uart_mmio_range(unsigned long far){
    if (far >= U_BASE && far <= U_IMSC_REG)
        return 1;
    else
        return 0;
}

int check_emul_mmio_range(unsigned long far){
    return check_uart_mmio_range(far);
}

static unsigned long emul_readreg(struct emu_mmio_access *emu_mmio, int reg)
{
    unsigned long val;

    if(reg < 0 || reg > 31)
        return 0;
    
    switch (emu_mmio->reg_width)
    {
    case 32:
		val = *(unsigned int *)((unsigned long)emu_mmio->regs + reg * 8);
		break;
    case 64:
		val = *(unsigned long *)((unsigned long)emu_mmio->regs + reg * 8);
        break;
    
    default:
        break;
    }
}

static void emul_write_addr(struct emu_mmio_access *emu_mmio, unsigned long var){
    switch (emu_mmio->width)
    {
        case 8:
            *((unsigned char*)emu_mmio->addr) = var;
            break;
        case 16:
            *((unsigned short*)emu_mmio->addr) = var;
            break;
        case 32:
            *((unsigned short*)emu_mmio->addr) = var;
            break;
        case 64:
            *((unsigned short*)emu_mmio->addr) = var;
            break;

        default:
            printk("%s unknown emu write size emu_mmio->width is %d\n", __func__, emu_mmio->width);
    }    

}

static unsigned long emul_read_addr(struct emu_mmio_access *emu_mmio){
    unsigned long val = 0;

    switch (emu_mmio->width){
        case 8:
            val = emu_mmio->sign_ext ? *((char *)emu_mmio->addr) : *((unsigned char*)emu_mmio->addr);
            break;
        case 16:
            val = emu_mmio->sign_ext ? *((short *)emu_mmio->addr) : *((unsigned short*)emu_mmio->addr);
            break;
        case 32:
            val = emu_mmio->sign_ext ? *((int *)emu_mmio->addr) : *((unsigned int*)emu_mmio->addr);
            break;
        case 64:
            val = emu_mmio->sign_ext ? *((long *)emu_mmio->addr) : *((unsigned long*)emu_mmio->addr);
            break;
        default:
            printk("%s unknown emu_mmio read size\n", __func__);
    }

    return val;
}

static void emul_writereg(struct emu_mmio_access *emu_mmio, int reg, unsigned long val)
{
    if(reg < 0 || reg > 31)
        return;
    switch (emu_mmio->reg_width)
    {
    case 32:
        *(unsigned int *)((unsigned long)emu_mmio->regs + reg * 8) = val;
        break;
    case 64:
        *(unsigned long *)((unsigned long)emu_mmio->regs + reg * 8) = val;
        break;
    default:
        break;
    }
}

static int emu_mmio_regs(struct emu_mmio_access *emu_mmio){
    unsigned long val;

    if(emu_mmio->write){
        val = emul_readreg(emu_mmio, emu_mmio->reg);
        emul_write_addr(emu_mmio, val);
    }else{
        val = emul_read_addr(emu_mmio);
        emul_writereg(emu_mmio, emu_mmio->reg, val);
    }

    return 0;
}

int emul_device(struct pt_regs *regs, unsigned long fault_addr,
        unsigned int esr)
{
    struct emu_mmio_access emu_mmio;
    unsigned long il, iss;
    unsigned long pc_step;

    il = (esr >> ESR_IL_OFFSET) & ESR_IL_MASK;

    iss = esr & ESR_ISS_MASK;


    /*judge invalid exception msg*/
    if(!(iss & ESR_ISS_DA_ISV_BIT) || (iss & ESR_ISS_DA_FnV_BIT))
        return -EFAULT;

    emu_mmio.regs = regs;
    emu_mmio.addr = fault_addr;

    /*width*/
    emu_mmio.width = (iss >> ESR_ISS_DA_SAS_SHIFT) & ESR_ISS_DA_SAS_MASK;
    emu_mmio.width = 8 * (1 << emu_mmio.width);


    emu_mmio.write = (iss & ESR_ISS_DA_WnR_BIT) ? 1:0;

    /* aid reg*/
	emu_mmio.reg = (iss >> ESR_ISS_DA_SRT_SHIFT) & ESR_ISS_DA_SRT_MASK;  

    /*aid reg width*/
    emu_mmio.reg_width = (iss & ESR_ISS_DA_SF_BIT) ? 64:32;

    /* ext */
    emu_mmio.sign_ext = (iss & ESR_ISS_DA_SSE_BIT) ? 1:0;

    emu_mmio_regs(&emu_mmio);

    pc_step = 2 + (2 * il);
    regs->pc += pc_step;

    return 0;
}